#ifndef __MPU9250_QUATERNIO_H__
#define __MPU9250_QUATERNIO_H__

#define PI 3.14159265358979323846f

// Set initial input parameters
typedef enum{
  AFS_2G = 0,
  AFS_4G,
  AFS_8G,
  AFS_16G
}Ascale_type;

typedef enum{
  GFS_250DPS = 0,
  GFS_500DPS,
  GFS_1000DPS,
  GFS_2000DPS
}Gscale_type;

typedef enum {
  MFS_14BITS = 0, // 0.6 mG per LSB
  MFS_16BITS      // 0.15 mG per LSB
}Mscale_type;

typedef struct
{
	signed char (*mpu9250_bus_write)(unsigned char addr, unsigned char reg, unsigned char *data, unsigned short len);
	signed char (*mpu9250_bus_read)(unsigned char addr, unsigned char reg, unsigned char *data, unsigned short len);
	void (*delayms)(unsigned int time);
	Ascale_type Ascale;
	Gscale_type Gscale;
	Mscale_type Mscale;
	unsigned char Mmode;

	// scale resolutions per LSB for the sensors
	float aRes;
	float gRes;
	float mRes;

}mpu9250_quaternio_t;

extern float deltat;


// extern unsigned char Ascale;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
// extern unsigned char Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
// extern unsigned char Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
// extern unsigned char Mmode;        // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR  

extern unsigned char mpu9250_quaternio_get_whoiam(mpu9250_quaternio_t *mpu);
extern signed char mpu9250_quaternio_reset(mpu9250_quaternio_t *mpu);
extern signed char mpu9250_quaternio_calibrate(mpu9250_quaternio_t *mpu, float *dest1, float *dest2);
extern signed char mpu9250_quaternio_self_test(mpu9250_quaternio_t *mpu, long *gyro, long *accel, unsigned char hw_test);
extern signed char mpu9250_quaternio_DevInit(mpu9250_quaternio_t *mpu);
extern signed char mpu9250_quaternio_MagDevInit(mpu9250_quaternio_t *mpu, float * destination);

extern float mpu9250_quaternio_getAres(Ascale_type scale);
extern float mpu9250_quaternio_getGres(Gscale_type scale);
extern float mpu9250_quaternio_getMres(Mscale_type scale);
extern void mpu9250_quaternio_update_all_res(mpu9250_quaternio_t *mpu);

extern signed char mpu9250_quaternio_get_INT_enable(mpu9250_quaternio_t *mpu);

extern signed char mpu9250_quaternio_read_Acc(mpu9250_quaternio_t *mpu, signed short *acc);
extern signed char mpu9250_quaternio_read_Gyro(mpu9250_quaternio_t *mpu, signed short *gyro);
extern signed char mpu9250_quaternio_read_Mag(mpu9250_quaternio_t *mpu, signed short *mag);
extern signed short mpu9250_quaternio_read_Temperature(mpu9250_quaternio_t *mpu);

extern void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float *q);

extern void inv_dmp_init(float *acc_b, float *gyro_b);
#endif
